
import lejos.nxt.*;
import lejos.robotics.navigation.*;
import lejos.robotics.navigation.Move.MoveType;
import lejos.robotics.localization.MCLParticleSet;
import lejos.robotics.mapping.*;
import lejos.geom.Line;
import lejos.geom.Rectangle;



public class PathFollow1 {
	
	static double SIDE_SIZE = 40.0;
	static double WHEEL_DIAMETER = 5.45f;
	static double AXIS_LENGTH = 11.2f;
	
	static float SQUARE_LENGTH = 50;
	
	public static void main(String[] args) throws InterruptedException {
		
		DifferentialPilot navigator = new DifferentialPilot(WHEEL_DIAMETER, AXIS_LENGTH, Motor.A, Motor.C);
		
		// We set the parameters to smooth the trajectory:
		navigator.setRotateSpeed(100);
		navigator.setTravelSpeed(100);
		navigator.setAcceleration(200);
		
		// Initialize the particle set:
		Line[] lines = {
				new Line(0,0,0,100),
				new Line(0,100,100,100),
				new Line(100,100,100,0),
				new Line(100,0,0,0)};

		MCLParticleSet particles = new MCLParticleSet(new LineMap(lines, new Rectangle(0, 0, 100, 100)), 100, 1);
		particles.setDistanceNoiseFactor(0.05f);
		particles.setAngleNoiseFactor(0.05f);
		for (int i = 0; i < particles.numParticles(); i++)
		{
			particles.getParticle(i).getPose().setLocation(10, 10);
			particles.getParticle(i).getPose().setHeading(0);
		}
		
		// Init the robot and the point:
		float myX = 10.f;
		float myY = 10.f;
		float myO = 0.f;
		
		float destX = 50.f;
		float destY = 50.f;
		float destO = 0.f;
		
		float[] vector = new float[]{destX - myX, destY - myY};
		float bearing = (float)Math.toDegrees(Math.atan2(vector[0], vector[1]));
		
		// First rotation
		navigator.rotate(bearing, true);

		Move movement;
		float lastA  = 0;
		float deltaA = 0;
		Move aux = new Move(0, 0, true);
		do
		{
			movement = navigator.getMovement();
			deltaA = movement.getAngleTurned() - lastA;
			lastA  = movement.getAngleTurned();
			aux.setValues(MoveType.ROTATE, 0, deltaA, true);
			particles.applyMove(aux);
			showParticles(particles);
		} while (movement.isMoving());

		// Update my orientation
		myO += navigator.getMovement().getAngleTurned();
		

		// Travel to point		
		float distance = (float)Math.sqrt(Math.pow(vector[0], 2) + Math.pow(vector[1], 2));
		float lastD = 0;
		float deltaD = 0;

		navigator.travel(distance, true);
		do
		{
			movement = navigator.getMovement();
			deltaD = movement.getDistanceTraveled() - lastD;
			lastD  = movement.getDistanceTraveled();
			aux.setValues(MoveType.TRAVEL, deltaD, 0, true);
			particles.applyMove(aux);
			showParticles(particles);
		} while (movement.isMoving());


		// Bearing to the final angle
		navigator.rotate(destO - myO, true);
		lastA = 0;
		do
		{
			movement = navigator.getMovement();
			deltaA = movement.getAngleTurned() - lastA;
			lastA  = movement.getAngleTurned();
			aux.setValues(MoveType.ROTATE, 0, deltaA, true);
			particles.applyMove(aux);
			showParticles(particles);
		} while (movement.isMoving());
		
		
		Button.waitForPress();
	}
	
	private static void showParticles(MCLParticleSet particles) throws InterruptedException
	{
		int offset = (LCD.SCREEN_WIDTH - LCD.SCREEN_HEIGHT) / 2;
		for (int i = 0; i < particles.numParticles(); i++)
		{
			int x = (int)(particles.getParticle(i).getPose().getX()/100 * LCD.SCREEN_HEIGHT);
			int y = (int)(particles.getParticle(i).getPose().getY()/100 * LCD.SCREEN_HEIGHT);
			LCD.setPixel(x + offset, y, 1);
		}
		
	}
}
